/*
 * InteractionCalculator_Lin.hpp
 *
 *  Created on: May 10, 2012
 *      Author: soniclin
 */

#ifndef INTERACTION_CALCULATOR_LIN_HPP
#define INTERACTION_CALCULATOR_LIN_HPP

#include "InteractionCalculator_Lin.h"
#include "./utilities/Utilities.h"
#include "./volumeFractionPolicy/Function1D.hpp"


template<typename DecType>
InteractionCalculator_Lin<DecType>::InteractionCalculator_Lin()
{
	stressProfile.open("ForceData.dat");
	counter = 0;

}

template<typename DecType>
InteractionCalculator_Lin<DecType>::InteractionCalculator_Lin(DecType tau)
{
	stressProfile.open("ForceData.dat");
	tau_ = tau;
	counter = 0;
}

template<typename DecType>
InteractionCalculator_Lin<DecType>::~InteractionCalculator_Lin()
{
	stressProfile.close();
}

template<typename DecType>
void InteractionCalculator_Lin<DecType>::clearAll()
{
	this->linearForce.clear();
	this->torque.clear();
	this->_sphereString.clear();

	sphere_forces.clear();
	sphere_torque.clear();

	normal_stress.clear();
	shear_stress.clear();

	sum_forces.clear();
	sum_torque.clear();

	sum_normal.clear();
	sum_shear.clear();

}

template<typename DecType>
unsigned InteractionCalculator_Lin<DecType>::getNumberOfDO() const
{
	return this->_sphereString.size();
}

template<typename DecType>
const NJRvector3d& InteractionCalculator_Lin<DecType>::getForces(Index sphereIndex) const
{
	return this->linearForce[sphereIndex];
}

template<typename DecType>
const NJRvector3d& InteractionCalculator_Lin<DecType>::getTorque(Index sphereIndex) const
{
	return this->torque[sphereIndex];
}

template<typename DecType>
sMathLib::Vector3& InteractionCalculator_Lin<DecType>::getNormalStress(unsigned sphereIndex) const
{
	return this->_sphereString[sphereIndex].getFluidEffect(NormalStress);
}

template<typename DecType>
sMathLib::Vector3& InteractionCalculator_Lin<DecType>::getShearStress(unsigned sphereIndex) const
{
	return this->_sphereString[sphereIndex].getFluidEffect(ShearStress);
}

template<typename DecType>
void InteractionCalculator_Lin<DecType>::recordFluidEffect(int iT, int recordStep, std::vector<DecType>& scalingFactor)
{
	if(singleton::mpi().getRank()==0)
	{
//		if(iT % recordStep == 0)
//		{
//			std::cout << "Recording Fluid effect...." << std::endl;
//			std::string fileName("DragForce_");
//
//			std::ostringstream osstr;
//
//			osstr << fileName << iT << ".dat";
//
//			fileName = osstr.str();
//
//			ImmersedObjectWriter<DecType, ibm::Sphere<DecType, ibm::ImmersedObject> > writer;
//
//			writer.BeginWriteObjects(fileName, _sphereString, 3);
//
//			writer.writeDataArray(NormalStress, _sphereString, "NormalStress", scalingFactor[NormalStress], 3);
//			writer.writeDataArray(ShearStress, _sphereString, "ShearStress", scalingFactor[ShearStress], 3);
//			writer.writeDataArray(DragForce, _sphereString, "DragForce", scalingFactor[DragForce], 3);
//
//			writer.EndWriteObjects(fileName, _sphereString, 3);
//		}
		stressProfile << iT << " ";

		for(unsigned dimIndex = 0; dimIndex != 3; ++dimIndex)
			stressProfile << _sphereString[0].getFluidEffect(NormalStress)[dimIndex] << " ";
		for(unsigned dimIndex = 0; dimIndex != 3; ++dimIndex)
			stressProfile << _sphereString[0].getFluidEffect(ShearStress)[dimIndex] << " ";
		for(unsigned dimIndex = 0; dimIndex != 3; ++dimIndex)
			stressProfile << _sphereString[0].getFluidEffect(DragForce)[dimIndex] << " ";
		stressProfile << std::endl;
	}

}

template<typename DecType>
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>::assignComputationalDomain( MultiBlockLattice3D<T, Lattice> &lattice )
{
	this->_computationalDomain[0] = lattice.getNx();
	this->_computationalDomain[1] = lattice.getNy();
	this->_computationalDomain[2] = lattice.getNz();

	_domainBox.x0 = 0; _domainBox.x1 = _computationalDomain.x();
	_domainBox.y0 = 0; _domainBox.y1 = _computationalDomain.y();
	_domainBox.z0 = 0; _domainBox.z1 = _computationalDomain.z();
}

template<typename DecType>
void InteractionCalculator_Lin<DecType>::assignBoundaries(int x0, int x1, int y0, int y1, int z0, int z1)
{

}

template<typename DecType>
void InteractionCalculator_Lin<DecType>::addDO(NJRvector3d particleCenter, NJRvector3d particleVelocity, NJRvector3d particleAV, DecType r)
{
	this->_sphereString.push_back( ibm::Sphere<DecType, ibm::ImmersedObject>(particleCenter, particleVelocity, particleAV, r) );
}

template<typename DecType>
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>::createVolumeFractionField(MultiBlockLattice3D<T, Lattice>& lattice)
{
	int iBlock = singleton::mpi().getRank();

	BlockParameters3D const& blockParam = lattice.getMultiData().getBlockParameters(iBlock);
	BlockLattice3D<T,Lattice> *localLattice = lattice.getBlockLattices()[iBlock];

	volumeFraction = new ibm::VolumeFractionField< std::map<int, T> >(localLattice->getNx(), localLattice->getNy(), localLattice->getNz());

}

#ifndef PARALLEL_MODE_MPI
//////////// Implementation by Sonic ////////////
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin::CalculatePhiAndModifyPopulation(BlockStructure3D<T, Lattice> &lattice, double drift, int iT)
{
	std::cout << "No parallel BlockLattice3D" << std::endl;
	/*
	int boundaryXRhs = lattice.getNx() - 1;
	int boundaryYRhs = lattice.getNy() - 1;
	int boundaryZRhs = lattice.getNz() - 1;

	int boundaryXLhs = 0;
	int boundaryYLhs = 0;
	int boundaryZLhs = 0;
	*/

	int Nx = lattice.getNx();

	for(int np = 0; np != this->global_p_center.size(); ++np)
	{
		double cx = this->global_p_center[np].x();
		double cy = this->global_p_center[np].y();
		double cz = this->global_p_center[np].z();

		int r_extend = (int)(this->global_p_r[np] +0.5) + 1;
		int d_extend = 2 * r_extend;

////////////////////// PBC //////////////////////

		int xLhs = max((int)cx - r_extend, boundaryX0);
		int xRhs = min(xLhs + d_extend, boundaryX1 - 1);

/*
		int xLhs = (int)(cx) - r_extend;
		int xRhs = xLhs + d_extend;
*/
////////////////////// PBC //////////////////////
		int yLhs = max((int)cy - r_extend, boundaryY0);
		int yRhs = min(yLhs + d_extend, boundaryY1 - 1);
		int zLhs = max((int)cz - r_extend, boundaryZ0);
		int zRhs = min(zLhs + d_extend, boundaryZ1 - 1);

		int lx = (int)cx - 10;
		int ly = (int)cy - 10;
		int lz = (int)cz - 10;

		using namespace std;

/*
			cout << endl;
			cout << xLhs << " " << xRhs << endl;
			cout << yLhs << " " << yRhs << endl;
			cout << zLhs << " " << zRhs << endl;
			cout << endl;
*/

		for(int x = xLhs; x != xRhs + 1; ++x)
		{
			for(int y = yLhs; y != yRhs + 1; ++y)
			{
				for(int z = zLhs; z != zRhs + 1; ++z)
				{
					double r = sqrt(((double)x - cx) * ((double)x - cx) + ((double)y - cy) * ((double)y - cy) + ((double)z - cz) * ((double)z - cz));
					double R = this->global_p_r[np];

					double dist = r - R;


					int ix = x - lx;
					int iy = y - ly;
					int iz = z - lz;

					double dr = drift;

					T phi = T();

					if(dist < -drift)
						phi = 1.;
					else if(dist < drift)
						phi = 1. - (dist + dr) / (dr + dr);
					else
						phi = 0.;

////////////////////// PBC //////////////////////
					/*
					if(x < 1)
					{
						x += Nx - 2;
					}
					else if(x > Nx - 2)
					{
						x -= Nx - 2;
					}
					else
					{

					}
					*/
///////////////////// PBC //////////////////////

					double Us[3] = {this->global_p_vel[np].x(), this->global_p_vel[np].y(), this->global_p_vel[np].z()};

					Cell<T, Lattice> &cell = lattice.get(x, y, z);

					cell.add_DOid_Phi(np, phi);

					T uSqr = olb::util::normSqr<T, 3>(Us);

					T density = cell.computeRho();

/*
					if(ix > 21 || iy > 21 || iz > 21)
					{
					std::cout << std::endl;
					std::cout << "///////////////////////////////////Sonic's CP at " << 4.31 << std::endl;
					std::cout << xLhs << " " << xRhs << std::endl;
					std::cout << yLhs << " " << yRhs << std::endl;
					std::cout << zLhs << " " << zRhs << std::endl;

					std::cout << "Sonic's CP at " << 4.3 << std::endl;
					std::cout << x << " " << y << " " << z << std::endl;

					std::cout << "ixiyiz = ";
					std::cout << ix << " " << iy << " " << iz << std::endl;

					std::cout << "cxcycz = ";
					std::cout << cx << " " << cy << " " << cz << std::endl;

					std::cout << "lxlylz = ";
					std::cout << lx << " " << ly << " " << lz << std::endl;

					std::cout << "xyz = ";
					std::cout << x << " " << y << " " << z << std::endl;

					std::cout << "Sonic's CP at " << 4.308 << std::endl;

					std::cout << x << " " << y << " " << z << std::endl;
					std::cout << "phi = " << phi << std::endl;
					std::cout << "Sonic's CP at " << 4.32	 << std::endl << std::endl;

					cout << "Np = " << np << endl;
				}
*/
					///Lattice<T>::q = 19 (D3Q19)
					/// The lattice populations are defined as a q-element C-array.
					/// typedef T fPop[Lattice<T>::q];
					/*
					if(iT == 4)
					{
						cout << "Us = " << Us[0] << " " << Us[1] << " " << Us[2] << endl;
						cout << "uSqr = " << uSqr << endl;
						cout << "density = " << density << endl;
						cout << "Start Modify f" << endl;
					}
					*/
					for (int iPop = 0; iPop != Lattice<T>::q; ++iPop)
					{
						cell[iPop] = (1. - phi) * cell[iPop] + phi * cell.computeEquilibrium(iPop, density, Us, uSqr);
					}

				}
			}
		}

	}

	//std::cout << "Sonic's CP at " << 4.5 << std::endl;
std::cout << "END No parallel BlockLattice3D 3" << std::endl;
}

template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>::CalculateForce(BlockStructure3D<T, Lattice> &lattice, double LatticeNu, double drift, int iT)
{
	std::cout << "Start calculate forces" << std::endl;
	std::vector<T> sphere_forces( this->global_p_center.size() * 3, T());
	std::vector<T> sphere_torque( this->global_p_center.size() * 3, T());
/*
	int boundaryXRhs = lattice.getNx() - 1;
	int boundaryYRhs = lattice.getNy() - 1;
	int boundaryZRhs = lattice.getNz() - 1;

	int boundaryXLhs = 0;
	int boundaryYLhs = 0;
	int boundaryZLhs = 0;
*/

	int Nx = lattice.getNx();
	//std::cout << "Calculator CP 1" << std::endl;
	for(int np = 0; np != this->global_p_center.size(); ++np)
	{
		//std::cout << "Calculator CP 2" << std::endl;
		double cx = this->global_p_center[np].x();
		double cy = this->global_p_center[np].y();
		double cz = this->global_p_center[np].z();

		//find bounding box
		int r_extend = (int)(this->global_p_r[np] +0.5) + 1;
		int d_extend = 2 * r_extend;
		//std::cout << "Calculator CP 3" << std::endl;
////////////////////// PBC //////////////////////

		int xLhs = max((int)cx - r_extend, boundaryX0 + 1);
		int xRhs = min(xLhs + d_extend, boundaryX1 - 1);
/*
		int xLhs = (int)(cx + 0.5) - r_extend;
		int xRhs = xLhs + d_extend;
*/
////////////////////// PBC //////////////////////

		int yLhs = max((int)cy - r_extend, boundaryY0 + 1);
		int yRhs = min(yLhs + d_extend, boundaryY1 - 1);
		int zLhs = max((int)cz - r_extend, boundaryZ0 + 1);
		int zRhs = min(zLhs + d_extend, boundaryZ1 - 1);

		//std::cout << "Calculator CP 4" << std::endl;

		int lx = (int)cx - 10;
		int ly = (int)cy - 10;
		int lz = (int)cz - 10;



		/*
		std::cout << "(xmin, xmax, ymin, ymax, zmin, zmax)" << std::endl;
		std::cout << xLhs << " " << xRhs << " " << yLhs << " " << yRhs << " " << zLhs << " " << zRhs << std::endl;
		*/

		map<int, map<int, map<int, NJRvector3d> > > U;
		map<int, map<int, map<int, double> > > P;

		//std::cout << "Calculator CP 5" << std::endl;
		/*
		std::cout << xLhs << " " << xRhs << endl;
		std::cout << yLhs << " " << yRhs << endl;
		std::cout << zLhs << " " << zRhs << endl;
		*/

		for(int x = xLhs - 1; x <= xRhs + 1; x++)
		{
			for(int y = yLhs - 1; y <= yRhs + 1; y++)
			{
				for(int z = zLhs - 1; z <= zRhs + 1; z++)
				{
////////////////////// PBC //////////////////////
					/*
					if(x < 1)
					{
						x += Nx - 2;
					}
					else if(x > Nx - 2)
					{
						x -= Nx - 2;
					}
					else
					{

					}
					*/
///////////////////// PBC //////////////////////
					Cell<T, Lattice> &cell = lattice.get(x, y, z);
					double u[3] = {0., 0., 0.};
					cell.computeU(u);
					double p = cell.computeRho() / Lattice<T>::invCs2;
					U[x][y][z] = NJRvector3d(u[0], u[1], u[2]);
					P[x][y][z] = p;
				}
			}
		}

		//std::cout << "Calculator CP 6" << std::endl;
		/*
		vector< vector< vector< NJRvector3d > > > U;
		U.assign(lattice.getNx()+1, vector< vector<NJRvector3d> >(lattice.getNy()+1, vector< NJRvector3d >(lattice.getNz()+1, NJRvector3d(0., 0., 0.))));

		vector< vector< vector< double > > > P;
		P.assign(lattice.getNx()+1, vector< vector<double> >(lattice.getNy()+1, vector< double >(lattice.getNz()+1, 0.)));

		for(int X = 0; X != lattice.getNx(); ++X)
		{
			for(int Y = 0; Y != lattice.getNy(); ++Y)
			{
				for(int Z = 0; Z != lattice.getNz(); ++Z)
				{
					double de = 0.;
					double u[3] = {0., 0., 0.};
					lattice.get(X, Y, Z).computeRhoU(de, u);

					U[X][Y][Z] = NJRvector3d(u[0], u[1], u[2]);
					P[X][Y][Z] = de / Lattice<T>::invCs2;
				}
			}
		}
		*/


		for(int x = xLhs; x <= xRhs; ++x)
		{
			for(int y = yLhs; y <= yRhs; ++y)
			{
				for(int z = zLhs; z <= zRhs; ++z)
				{
					//query phi
					int ix = x - lx;
					int iy = y - ly;
					int iz = z - lz;
					Cell<T, Lattice> &cell = lattice.get(x, y, z);
					T phi = cell.queryPhi(np);

////////////////////// PBC //////////////////////
					/*
					if(x < 1)
					{
						x += Nx - 2;
					}
					else if(x > Nx - 2)
					{
						x -= Nx - 2;
					}
					else
					{

					}
					*/
///////////////////// PBC //////////////////////

					NJRvector3d u_IiJjKk = U[x][y][z];
					NJRvector3d u_IpJjKk = U[x+1][y][z];
					NJRvector3d u_ImJjKk = U[x-1][y][z];
					NJRvector3d u_IiJpKk = U[x][y+1][z];
					NJRvector3d u_IiJmKk = U[x][y-1][z];
					NJRvector3d u_IiJjKp = U[x][y][z+1];
					NJRvector3d u_IiJjKm = U[x][y][z-1];

					double UXX = LatticeNu * (u_IpJjKk.x() - 2.*u_IiJjKk.x() + u_ImJjKk.x());
					double UYY = LatticeNu * (u_IiJpKk.x() - 2.*u_IiJjKk.x() + u_IiJmKk.x());
					double UZZ = LatticeNu * (u_IiJjKp.x() - 2.*u_IiJjKk.x() + u_IiJjKm.x());
					double VXX = LatticeNu * (u_IpJjKk.y() - 2.*u_IiJjKk.y() + u_ImJjKk.y());
					double VYY = LatticeNu * (u_IiJpKk.y() - 2.*u_IiJjKk.y() + u_IiJmKk.y());
					double VZZ = LatticeNu * (u_IiJjKp.y() - 2.*u_IiJjKk.y() + u_IiJjKm.y());
					double WXX = LatticeNu * (u_IpJjKk.z() - 2.*u_IiJjKk.z() + u_ImJjKk.z());
					double WYY = LatticeNu * (u_IiJpKk.z() - 2.*u_IiJjKk.z() + u_IiJmKk.z());
					double WZZ = LatticeNu * (u_IiJjKp.z() - 2.*u_IiJjKk.z() + u_IiJjKm.z());

					double p_IpJjKk = P[x+1][y][z];
					double p_ImJjKk = P[x-1][y][z];
					double p_IiJpKk = P[x][y+1][z];
					double p_IiJmKk = P[x][y-1][z];
					double p_IiJjKp = P[x][y][z+1];
					double p_IiJjKm = P[x][y][z-1];

					double DPX = 0.5 * (p_IpJjKk - p_ImJjKk);
					double DPY = 0.5 * (p_IiJpKk - p_IiJmKk);
					double DPZ = 0.5 * (p_IiJjKp - p_IiJjKm);

					/*
					double UXX = LatticeNu * (U[x+1][y][z].x() - 2. * U[x][y][z].x() + U[x-1][y][z].x());
					double UYY = LatticeNu * (U[x][y+1][z].x() - 2. * U[x][y][z].x() + U[x][y-1][z].x());
					double UZZ = LatticeNu * (U[x][y][z+1].x() - 2. * U[x][y][z].x() + U[x][y][z-1].x());

					double VXX = LatticeNu * (U[x+1][y][z].y() - 2. * U[x][y][z].y() + U[x-1][y][z].y());
					double VYY = LatticeNu * (U[x][y+1][z].y() - 2. * U[x][y][z].y() + U[x][y-1][z].y());
					double VZZ = LatticeNu * (U[x][y][z+1].y() - 2. * U[x][y][z].y() + U[x][y][z-1].y());

					double WXX = LatticeNu * (U[x+1][y][z].z() - 2. * U[x][y][z].z() + U[x-1][y][z].z());
					double WYY = LatticeNu * (U[x][y+1][z].z() - 2. * U[x][y][z].z() + U[x][y-1][z].z());
					double WZZ = LatticeNu * (U[x][y][z+1].z() - 2. * U[x][y][z].z() + U[x][y][z-1].z());

					double DPX = 0.5 * (P[x+1][y][z] - P[x-1][y][z]);
					double DPY = 0.5 * (P[x][y+1][z] - P[x][y-1][z]);
					double DPZ = 0.5 * (P[x][y][z+1] - P[x][y][z-1]);
					*/
					sphere_forces[np * 3]     += phi * (UXX + UYY + UZZ - DPX);
					sphere_forces[np * 3 + 1] += phi * (VXX + VYY + VZZ - DPY);
					sphere_forces[np * 3 + 2] += phi * (WXX + WYY + WZZ - DPZ);
				}
			}
		}
		//std::cout << "Calculator CP 7" << std::endl;
	}
	/*
	ostringstream IT;
	IT << iT;
	string filename("U_");
	string foot(".dat");
	string filename2 = filename + IT.str() + foot;
	std::ofstream velField(filename2.c_str());
	velField << "variables=x,z,u,v, P" << endl;
	velField << "zone i=" << lattice.getNx() << " ,j=" << lattice.getNz() << " ,f=point" << endl;
	for(int Z = 0; Z != lattice.getNz(); ++Z)
	{
		for(int X = 0; X != lattice.getNx(); ++X)
		{
			double u[3] = {0, 0, 0};
			lattice.get(X, 50, Z).computeU(u);
			double P = lattice.get(X, 50, Z).computeRho() / 3.;
			velField << X << " " << Z << " " << u[0] << " " << u[1] << " " << P << std::endl;
		}
	}
	velField.close();
	*/
	//std::cout << "Calculator CP 8" << std::endl;
	for(int np = 0; np != this->global_p_center.size(); ++np)
	{
		this->linearForce.push_back(NJRvector3d(sphere_forces[np * 3], sphere_forces[np * 3 + 1], sphere_forces[np * 3 + 2]));
		this->torque.push_back(NJRvector3d(0., 0., 0.));
	}
	std::cout << "END calculate forces" << std::endl;
}

template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>::CalculateInteraction(BlockStructure3D<T, Lattice>& lattice, double lat_nu, int iT)
{
	lattice.forAll(ClearPhiF<T, Lattice>());
	this->CalculatePhiAndModifyPopulation(lattice, -0.5, iT);
	this->CalculateForce(lattice, lat_nu, -0.5, iT);
	std::cout << "Leave Interaction " << endl;
}
//////////// Implementation by Sonic ////////////
#endif

//////////// Implementation by liao (modified by sonic)////////////
template<typename DecType>
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>:: calculateVfAndModifyPopulation(MultiBlockLattice3D<T, Lattice> &lattice, int iT)
{
	int iBlock = singleton::mpi().getRank();
	BlockParameters3D const& blockParam = lattice.getMultiData().getBlockParameters(iBlock);
	BlockCoordinates3D const& envelope = blockParam.getEnvelope();
	BlockLattice3D<T,Lattice> *localLattice = lattice.getBlockLattices()[iBlock];

	if(iBlock == singleton::mpi().isMainProcessor())
		std::cout << "==|Parallel IB: Start to Define Volume fraction function and Modify field" << std::endl;

	volumeFraction->reset();


	T drift_in = 0.5;
	T drift_out = 0.5;


//	/// Owen's method
//	std::vector<T> sphere_forces( this->_sphereString.size() * 3, 0.);
//	std::vector<T> sphere_torque( this->_sphereString.size() * 3, 0.);

	for(int np=0; np != this->_sphereString.size(); ++np)
	{
		//find bounding box
		ibm::Sphere<DecType, ibm::ImmersedObject>& sphere = _sphereString[np];
		T sRadius = sphere.getRadius();
		const sMathLib::VectorX<DecType, 3>& sphereVel = sphere.getVelocity();
		const sMathLib::VectorX<DecType, 3>& sphereAVel = sphere.getAngularVelocity();

		// Create Sphere bounding box for intersection
		sphere.createBoundingBox(drift_in, drift_out);
		sphere.generateShiftedSpheres(_domainBox);
		sphere.createBoundingBoxForShiftedSphere(drift_in, drift_out);

		for(int iSphere = 0; iSphere != sphere.getShiftedSize(); ++iSphere)
		{

			Box3D<T>& bounding = sphere.getShiftedBounding(iSphere);
			sMathLib::Vector3& sPos = sphere.getShiftedPosition(iSphere);

			int int_X0, int_X1, int_Y0, int_Y1, int_Z0, int_Z1;
			if(util::intersect(
				envelope.x0, envelope.x1, envelope.y0, envelope.y1, envelope.z0, envelope.z1,
				bounding.x0, bounding.x1, bounding.y0, bounding.y1, bounding.z0, bounding.z1,
				int_X0, int_X1, int_Y0, int_Y1, int_Z0, int_Z1
				))
			{
				for(int iX = int_X0; iX != int_X1 + 1; ++iX)
				{
					for(int iY = int_Y0; iY != int_Y1 + 1; ++iY)
					{
						for(int iZ = int_Z0; iZ != int_Z1 + 1; ++iZ)
						{
//							sMathLib::VectorX<T, 3> rVector(iX-sPos.x(), iY-sPos.y(), iZ-sPos.z());
//

//							T r = rVector.Abs();
//
//							//calculate phi
//							T dist = r - sRadius;
//							T phi = dist < -drift_in ? 1.0 : (dist < drift_out ? 1.-(dist + drift_in)/(drift_in + drift_out) : 0.);

							sMathLib::VectorX<int , 3> localPos;
							localPos[0] = blockParam.toLocalX(iX);
							localPos[1] = blockParam.toLocalY(iY);
							localPos[2] = blockParam.toLocalZ(iZ);

							const sMathLib::VectorX<int, 3> iPos(iX, iY, iZ);
							const T phi = ibm::Utilities::calculateVolumeFraction(iPos, sphere, ibm::Function1D<DecType>());

							Cell<T,Lattice>& cell = localLattice->get(localPos.x(), localPos.y(), localPos.z());
							volumeFraction->get(localPos.x(), localPos.y(), localPos.z())[np] = phi;

							const T density = cell.computeRho();

							sMathLib::VectorX<T, 3> rVector(iPos.x()-sphere.getPosition().x(), iPos.y()-sphere.getPosition().y(), iPos.z()-sphere.getPosition().z());
							sMathLib::Vector3 tangentialVelocity = sMathLib::cross(sphereAVel,rVector);
							T Us[3] = {sphereVel.x() + tangentialVelocity[0],
							           sphereVel.y() + tangentialVelocity[1],
							           sphereVel.z() + tangentialVelocity[2]};

							const T uSqr = util::normSqr<T,Lattice<T>::d>(Us);

							for (int iPop=0; iPop != Lattice<T>::q; ++iPop)
								cell[iPop] = (1. - phi) * cell[iPop] + phi * cell.computeEquilibrium(iPop, density, Us, uSqr);

							/// Owen's method
//							T beta = ( phi*(tau_-0.5) ) / ( (1+phi) + ( tau_-0.5 ) );
//
//							T omega = 1. / tau_;
//
//							const int half_q = 7;//Lattice<T>::q / 2;
//
//							T uf[3]  = {0, 0, 0};
//
//							cell.computeU(uf);
//
//							const T ufSqr = util::normSqr<T,Lattice<T>::d>(uf);
//							const T usSqr = util::normSqr<T,Lattice<T>::d>(Us);
//
//							T f_m[Lattice<T>::q];
//
//							T dragX = 0; T dragY = 0; T dragZ = 0;
//
//							for (int iPop=1; iPop != Lattice<T>::q; ++iPop)
//							{
//								f_m[iPop] = cell[iPop+half_q] - cell[iPop]
//								      + cell.computeEquilibrium(iPop, density, Us, usSqr);
//								      - cell.computeEquilibrium(iPop+half_q, density, uf, ufSqr);
//
//								cell.set_fBack( iPop,
//										beta * ( omega*(cell[iPop] - cell.computeEquilibrium(iPop, density, uf, ufSqr)) + f_m[iPop])
//										);
//
//								dragX += f_m[iPop]*Lattice<T>::c[iPop][0];
//								dragY += f_m[iPop]*Lattice<T>::c[iPop][1];
//								dragZ += f_m[iPop]*Lattice<T>::c[iPop][2];
//							}
//
//							dragX *= beta; dragY *= beta; dragZ *= beta;
//
//							T c = 1e-3/2.720030029e-4;
//
//
//							sphere_forces[np * 3]     += c*1e-3*1e-3*dragX;
//							sphere_forces[np * 3 + 1] += c*1e-3*1e-3*dragY;
//							sphere_forces[np * 3 + 2] += c*1e-3*1e-3*dragZ;
//
//							T torqueX = rVector.y()*dragZ - rVector.z()*dragY;
//							T torqueY = rVector.z()*dragX - rVector.x()*dragZ;
//							T torqueZ = rVector.x()*dragY - rVector.y()*dragX;
//
//							sphere_torque[np * 3]     += c*1e-3*1e-3*torqueX;
//							sphere_torque[np * 3 + 1] += c*1e-3*1e-3*torqueY;
//							sphere_torque[np * 3 + 2] += c*1e-3*1e-3*torqueZ;
//
//							///
						}// ix
					}// iy
				}// iz [

			} // intersect
		}// iSphere

	}// np


//	std::vector<T> sum_forces(this->_sphereString.size() * 3);
//	singleton::mpi().allReduceVect(sphere_forces, sum_forces, MPI_SUM);
//
//	std::vector<T> sum_torque(this->_sphereString.size() * 3);
//	singleton::mpi().allReduceVect(sphere_torque, sum_torque, MPI_SUM);
//
//	this->linearForce.clear();
//	this->torque.clear();
//
//	for(int np = 0; np != this->_sphereString.size(); ++np)
//	{
//		this->linearForce.push_back(NJRvector3d(-sum_forces[np * 3], -sum_forces[np * 3 + 1], -sum_forces[np * 3 + 2]));
//		this->torque.push_back(NJRvector3d(sum_torque[np * 3], sum_torque[np * 3 + 1], sum_torque[np * 3 + 2]));
//		//this->torque.push_back(NJRvector3d(0., 0., 0.));
//	}
if(iBlock == singleton::mpi().isMainProcessor())
	std::cout << "==|Parallel IB: End Define Volume fraction function and Modify field" << std::endl;
}

template<typename DecType>
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>::calculateForce(MultiBlockLattice3D<T, Lattice> &lattice, T LatticeNu, T drift_in, T drift_out, int iT)
{


	int iBlock = singleton::mpi().getRank();
	BlockParameters3D const &blockParam = lattice.getMultiData().getBlockParameters(iBlock);
	BlockCoordinates3D const &bulk = blockParam.getBulk();
	BlockCoordinates3D const& envelope = blockParam.getEnvelope();
	BlockLattice3D<T,Lattice> *localLattice = lattice.getBlockLattices()[iBlock];

	if(iBlock == singleton::mpi().isMainProcessor())
			std::cout << "==|Parallel IB: Start Calculate force and torque " << std::endl;


	sphere_forces.assign( this->_sphereString.size() * 3, 0.);
	sphere_torque.assign( this->_sphereString.size() * 3, 0.);

	normal_stress.assign( this->_sphereString.size() * 3, 0.);
	shear_stress.assign( this->_sphereString.size() * 3, 0.);

	T driftP = std::abs(drift_in);

	for(int np=0; np != this->_sphereString.size(); np++)
	{
		ibm::Sphere<DecType, ibm::ImmersedObject>& sphere = _sphereString[np];
		int counter = 0;
		for(int iSphere = 0; iSphere != sphere.getShiftedSize(); ++iSphere)
		{
			//find bounding box
			Box3D<DecType>& bounding = sphere.getShiftedBounding(iSphere);
			sMathLib::VectorX<T, 3>& spherePos = sphere.getShiftedPosition(iSphere);

			BlockCoordinates3D const& envelope = blockParam.getEnvelope();
			sMathLib::VectorX<T, 3> local_sPos = spherePos.shift(static_cast<T>(-envelope.x0), static_cast<T>(-envelope.y0), static_cast<T>(-envelope.z0));

			int int_X0, int_X1, int_Y0, int_Y1, int_Z0, int_Z1;
			if(util::intersect(
				bulk.x0, bulk.x1, bulk.y0, bulk.y1, bulk.z0, bulk.z1,
				bounding.x0, bounding.x1, bounding.y0, bounding.y1, bounding.z0, bounding.z1,
				int_X0, int_X1, int_Y0, int_Y1, int_Z0, int_Z1
				))
			{
				BlockCoordinates3D const local_lattice_coord(
						blockParam.toLocalX(int_X0), blockParam.toLocalX(int_X1),
						blockParam.toLocalY(int_Y0), blockParam.toLocalY(int_Y1),
						blockParam.toLocalZ(int_Z0), blockParam.toLocalZ(int_Z1)
						);

				// calculate require information
				std::map<int, std::map<int, std::map<int, NJRvector3d> > > velocity_field;
				std::map<int, std::map<int, std::map<int, T> > > pressure_field;

				T density = 0;
				T u[3] = {T(), T(), T()};

				for(int iX = local_lattice_coord.x0-1; iX <= local_lattice_coord.x1+1; iX++)
				{
					for(int iY = local_lattice_coord.y0-1; iY <= local_lattice_coord.y1+1; iY++)
					{
						for(int iZ = local_lattice_coord.z0-1; iZ <= local_lattice_coord.z1+1; iZ++)
						{
							Cell<T,Lattice>& cell = localLattice->get(iX, iY, iZ);

							cell.computeRhoU(density, u);

							velocity_field[iX][iY][iZ] = NJRvector3d(u[0],u[1],u[2]);
							pressure_field[iX][iY][iZ] = density / Lattice<T>::invCs2;
						}
					}
				}


				for(int iX = local_lattice_coord.x0; iX <= local_lattice_coord.x1; iX++)
				{
					for(int iY = local_lattice_coord.y0; iY <= local_lattice_coord.y1; iY++)
					{
						for(int iZ = local_lattice_coord.z0; iZ <= local_lattice_coord.z1; iZ++)
						{
							//query phi
							Cell<T,Lattice>& cell = localLattice->get(iX, iY, iZ);
							double phi = volumeFraction->get(iX, iY, iZ)[np];

							// query velocity field
							NJRvector3d& u_IiJjKk = velocity_field[iX][iY][iZ];
							NJRvector3d& u_IpJjKk = velocity_field[iX+1][iY][iZ];
							NJRvector3d& u_ImJjKk = velocity_field[iX-1][iY][iZ];
							NJRvector3d& u_IiJpKk = velocity_field[iX][iY+1][iZ];
							NJRvector3d& u_IiJmKk = velocity_field[iX][iY-1][iZ];
							NJRvector3d& u_IiJjKp = velocity_field[iX][iY][iZ+1];
							NJRvector3d& u_IiJjKm = velocity_field[iX][iY][iZ-1];

							T UXX = u_IpJjKk.x() - 2.*u_IiJjKk.x() + u_ImJjKk.x();
							T UYY = u_IiJpKk.x() - 2.*u_IiJjKk.x() + u_IiJmKk.x();
							T UZZ = u_IiJjKp.x() - 2.*u_IiJjKk.x() + u_IiJjKm.x();

							T VXX = u_IpJjKk.y() - 2.*u_IiJjKk.y() + u_ImJjKk.y();
							T VYY = u_IiJpKk.y() - 2.*u_IiJjKk.y() + u_IiJmKk.y();
							T VZZ = u_IiJjKp.y() - 2.*u_IiJjKk.y() + u_IiJjKm.y();

							T WXX = u_IpJjKk.z() - 2.*u_IiJjKk.z() + u_ImJjKk.z();
							T WYY = u_IiJpKk.z() - 2.*u_IiJjKk.z() + u_IiJmKk.z();
							T WZZ = u_IiJjKp.z() - 2.*u_IiJjKk.z() + u_IiJjKm.z();

							// query pressure field
							T& p_IpJjKk = pressure_field[iX+1][iY][iZ];
							T& p_ImJjKk = pressure_field[iX-1][iY][iZ];
							T& p_IiJpKk = pressure_field[iX][iY+1][iZ];
							T& p_IiJmKk = pressure_field[iX][iY-1][iZ];
							T& p_IiJjKp = pressure_field[iX][iY][iZ+1];
							T& p_IiJjKm = pressure_field[iX][iY][iZ-1];

							// Calculate pressure gradient
							sMathLib::VectorX<T, 3> normalStress;
							normalStress[0] = 0.5 * (p_IpJjKk - p_ImJjKk);
							normalStress[1] = 0.5 * (p_IiJpKk - p_IiJmKk);
							normalStress[2] = 0.5 * (p_IiJjKp - p_IiJjKm);

							sMathLib::VectorX<T, 3> shearStress;
							shearStress[0] = LatticeNu*(UXX+UYY+UZZ);
							shearStress[1] = LatticeNu*(VXX+VYY+VZZ);
							shearStress[2] = LatticeNu*(WXX+WYY+WZZ);

							// calculate drag force at this cell
							sMathLib::VectorX<T, 3> dragForce(
							shearStress[0] - normalStress.x(),
							shearStress[1] - normalStress.y(),
							shearStress[2] - normalStress.z());

							normal_stress[np * 3]     -= phi * normalStress.x();
							normal_stress[np * 3 + 1] -= phi * normalStress.y();
							normal_stress[np * 3 + 2] -= phi * normalStress.z();

							shear_stress[np * 3]     += phi * shearStress.x();
							shear_stress[np * 3 + 1] += phi * shearStress.y();
							shear_stress[np * 3 + 2] += phi * shearStress.z();

							// summation drag force
							sphere_forces[np * 3]     += phi * dragForce.x();
							sphere_forces[np * 3 + 1] += phi * dragForce.y();
							sphere_forces[np * 3 + 2] += phi * dragForce.z();

							sMathLib::VectorX<T, 3> rVector((T)iX - local_sPos.x(), (T)iY - local_sPos.y(), (T)iZ - local_sPos.z());

							T DUY = 0.5 * (u_IiJpKk.x() - u_IiJmKk.x());
							T DUZ = 0.5 * (u_IiJjKp.x() - u_IiJjKm.x());

							T DVX = 0.5 * (u_IpJjKk.y() - u_ImJjKk.y());
							T DVZ = 0.5 * (u_IiJjKp.y() - u_IiJjKm.y());

							T DWX = 0.5 * (u_IpJjKk.z() - u_ImJjKk.z());
							T DWY = 0.5 * (u_IiJpKk.z() - u_IiJmKk.z());

							sMathLib::VectorX<T, 3> dragTorque = sMathLib::cross(rVector, dragForce);

							sphere_torque[np * 3]     += phi * (dragTorque.x() + (DWY - DVZ)*LatticeNu);
							sphere_torque[np * 3 + 1] += phi * (dragTorque.y() + (DUZ - DWX)*LatticeNu);
							sphere_torque[np * 3 + 2] += phi * (dragTorque.z() + (DVX - DUY)*LatticeNu);
						}// iZ
					}// iY
				}// iX

				++counter;
			} //if(util::intersect
		} // iSphere
	} // for(int np=0; np<this->global_p_center.size(); np++)

	std::vector<T> sum_forces(this->_sphereString.size() * 3);
	ibm::MpiFacade::allReduceVect(sphere_forces, sum_forces, MPI_SUM);

	std::vector<T> sum_torque(this->_sphereString.size() * 3);
	ibm::MpiFacade::allReduceVect(sphere_torque, sum_torque, MPI_SUM);

	std::vector<T> sum_normal(this->_sphereString.size() * 3);
	ibm::MpiFacade::allReduceVect(normal_stress, sum_normal, MPI_SUM);

	std::vector<T> sum_shear(this->_sphereString.size() * 3);
	ibm::MpiFacade::allReduceVect(shear_stress, sum_shear, MPI_SUM);

	this->linearForce.clear();
	this->torque.clear();

	for(int np = 0; np != this->_sphereString.size(); ++np)
	{
		this->linearForce.push_back(NJRvector3d(sum_forces[np * 3], sum_forces[np * 3 + 1], sum_forces[np * 3 + 2]));
			 this->torque.push_back(NJRvector3d(sum_torque[np * 3], sum_torque[np * 3 + 1], sum_torque[np * 3 + 2]));

		sMathLib::Vector3 normalStress(sum_normal[np * 3], sum_normal[np * 3+1], sum_normal[np * 3+2]);
		sMathLib::Vector3 shearStress(sum_shear[np * 3], sum_shear[np * 3+1], sum_shear[np * 3+2]);
		sMathLib::Vector3 dragForce(sum_forces[np * 3], sum_forces[np * 3+1], sum_forces[np * 3+2]);

		_sphereString[np].recordFluidEffect(normalStress);
		_sphereString[np].recordFluidEffect(shearStress);
		_sphereString[np].recordFluidEffect(dragForce);
//		this->torque.push_back(NJRvector3d(0., 0., 0.));
	} // for(int np = 0; np != this->global_p_center.size(); ++np)

	if(iBlock == singleton::mpi().isMainProcessor())
		std::cout << "==|Parallel IB: End Calculate force and torque " << std::endl;
}

template<typename DecType>
template<typename T, template<typename U> class Lattice>
void InteractionCalculator_Lin<DecType>::parallelCalculateInteraction(MultiBlockLattice3D<T, Lattice>& lattice, T Nu, int iT)
{
	int iBlock = singleton::mpi().getRank();
	std::clock_t ict0 = std::clock();

	std::clock_t ict1 = std::clock();
	// Compute phi
	calculateVfAndModifyPopulation(lattice, iT);
	//this->calculateVfAndModifyPopulation(lattice, -0.3, 0., iT);

	std::clock_t ict2 = std::clock();
	if(iBlock == singleton::mpi().isMainProcessor())
		std::cout << "=|Calculate phi and modify fluid field, Iteraction Time 2 = " << (double)(ict2 - ict1) / (double)CLOCKS_PER_SEC << " sec" << std::endl;

	// sum forces & torque
	this->calculateForce(lattice, Nu, 0., 1.5, iT);

	std::clock_t ict3 = std::clock();
	if(iBlock == singleton::mpi().isMainProcessor())
		std::cout << "=|Calculate force, Iteraction Time 3 = " << (double)(ict3 - ict2) / (double)CLOCKS_PER_SEC << " sec" << std::endl;
}
//////////// Implementation by liao (modified by sonic)////////////

////////////// Implementation by Sonic ////////////
//template<typename T, template<typename U> class Lattice>
//void InteractionCalculator_Lin::CalculateForceFromMomentumFlux(MultiBlockLattice3D<T, Lattice> &lattice, T LatticeNu, T drift_in, T drift_out, int iT)
//{
//	std::cout << "==|Parallel IB: Start Calculate force and torque " << std::endl;
//
//		int iBlock = singleton::mpi().getRank();
//		BlockParameters3D const &blockParam = lattice.getMultiData().getBlockParameters(iBlock);
//		BlockCoordinates3D const &bulk = blockParam.getBulk();
//		BlockLattice3D<T,Lattice> *localLattice = lattice.getBlockLattices()[iBlock];
//
//		std::vector<T> sphere_forces( this->_sphereString.size() * 3, 0.);
//		std::vector<T> sphere_torque( this->_sphereString.size() * 3, 0.);
//
//		T driftP = std::abs(drift_in);
//}
////////////// Implementation by Sonic ////////////



#endif /* INTERACTION_CALCULATOR_LIN_HPP_ */
